#!/usr/bin/env python
#-*- coding:utf-8   -*-
import time
import os, sys
import rospy
import actionlib
import action_server.msg
from AutoPower import Obstacle
from geometry_msgs.msg import Twist
from RosSlam import RosC
dir_mytest = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
sys.path.insert(0, dir_mytest)
from log.log import initLogConf

#1.新对桩 sukai 2022-10-24
from contnav_srvs.srv import chargeMove, chargeMoveRequest, chargeMoveResponse


'''
action： action_server_power
消息：
AutoPower.action
    string order
    ---
    string result
    ---
    string feedback

order == "move_in"
order == "move_out"
order == "move_stop" ##sukai 后加

order == "close_slam"
order == "nav"
order == "close_nav"
order == "close_nav"


sukai 检查这个里面的 charge_action_callback
pm_server/src/main.cpp:2253:	charger_action = n.subscribe("/action_server_power/result", 10, charge_action_callback);
	if (opt == 0)
		goal.order = "move_in";
	else if (opt == 1)
		goal.order = "move_out";
	else if (opt == 2)
		goal.order = "move_turn:180";
	else if (opt == 3)
		goal.order = "slam";
	else if (opt == 4)
		goal.order = "close_slam";
	else if (opt == 5)
		goal.order = "nav";
	else if (opt == 6)
		goal.order = "close_nav";
		
	if (opt == 0 || opt == 1 || opt == 2)
	{
		ROS_WARN("send_move_task_action--2...");/*2022-09-16*/
		while (!ros::isShuttingDown() && (Main_task_excute_thread_flag == 1 || Main_task_back_excute_flag == 1))
		{
			if (charge_result_str == "success")
			{
				charge_result_str = "";
				ROS_WARN("Send Goal Success!");/*2022-09-17*/
				break;
			}
			else if (charge_result_str == "false")
			{
				charge_result_str = "";
				ROS_WARN("Send Goal False!");/*2022-09-17*/
				return false;
			}
			else
			{
				std::this_thread::sleep_for(std::chrono::milliseconds(500));
				continue;
			}
		}
	}		

'''



class Action_Power(object):
    # create messages that are used to publish feedback/result
    _feedback = action_server.msg.AutoPowerActionFeedback()
    _result = action_server.msg.AutoPowerActionResult()

    def __init__(self, name):
        rospy.init_node('python_c')
        self.loger = initLogConf("rospy_action_server")
        self._action_name = name
        self.ros_c = RosC()
        self.nav_status = False
        rospy.on_shutdown(self.shutdown)
        self._cmd_pub = rospy.Publisher("cmd_vel", Twist, queue_size=1)
        self._as = actionlib.SimpleActionServer(self._action_name, action_server.msg.AutoPowerAction, execute_cb=self.execute_cb, auto_start = False)
        self._as.start()
        self.loger.info("action_server start")

        #2.创建Service Client  新对桩 sukai 2022-10-24
        serviceName = "/chargeMove"
        self.chargeMoveClient = rospy.ServiceProxy(serviceName, chargeMove)
        # 等待服务开启
        #rospy.wait_for_service(serviceName)
        
    def execute_cb(self, goal):
        # helper variables
        obstacle = Obstacle()
        self._result.result = ''
        if goal.order == "move_in":
            self.loger.info("move_in")
            #3.创建Service Client  新对桩 sukai 2022-10-24  底盘控制，自动对接充电
            # 创建请求数据
            request = chargeMoveRequest()
            #充电桩模型匹配完成 AddTwoIntsRequest
            request.request_type = 'startchargeing' ## 开始对桩
            # 调用服务并且获得响应结果  AddTwoIntsResponse
            response = self.chargeMoveClient.call(request)

            if isinstance(response, chargeMoveResponse):
                rospy.loginfo("响应结果startchargeing: %s" % response.result)
                if response.response.result!="ok" :
                    self._result.result = 'false'
                else:
                    self._result.result = 'success'
            self._as.set_succeeded(self._result)

            ## 原数据 old
            '''
                        if obstacle.charging():
                self._result.result = 'success'
            else:
                self._result.result = 'false'
                self.loger.info("%s false", "move_in")

            self._as.set_succeeded(self._result)
            
            
            '''


        elif goal.order == "move_out":
            #4.创建Service Client  新对桩 sukai 2022-10-24  底盘控制，自动对接充电
            # 创建请求数据
            request = chargeMoveRequest()
            #充电桩模型匹配完成 AddTwoIntsRequest
            request.request_type = 'chargeingOff'
            # 调用服务并且获得响应结果  AddTwoIntsResponse
            response = self.chargeMoveClient.call(request)

            if isinstance(response, chargeMoveResponse):
                rospy.loginfo("响应结果chargeingOff: %s" % response.result)
                if response.response.result!="ok" :
                    self._result.result = 'false'
                else:
                    self._result.result = 'success'
            self._as.set_succeeded(self._result)

            ## 原数据 old
            '''
                self.loger.info("move_out")
            if obstacle.move_out():
                self._result.result = 'success'
            else:
                self._result.result = 'false'
                self.loger.info("%s false", "move_out")
            self._as.set_succeeded(self._result)
            
            '''
        elif goal.order == "move_stop":  ###停止对桩
            #5.创建Service Client  新对桩 sukai 2022-10-24  底盘控制，自动对接充电
            # 创建请求数据
            request = chargeMoveRequest()
            #充电桩模型匹配完成 AddTwoIntsRequest
            request.request_type = 'chargeingStop'
            # 调用服务并且获得响应结果  AddTwoIntsResponse
            response = self.chargeMoveClient.call(request)

            if isinstance(response, chargeMoveResponse):
                rospy.loginfo("响应结果chargeingStop: %s" % response.result)
                if response.response.result!="ok" :
                    self._result.result = 'false'
                else:
                    self._result.result = 'success'
            self._as.set_succeeded(self._result)

        
        elif goal.order[0:9] == "move_turn":
            self.loger.info("move_turn:-----%s", goal.order[10:]) 
            if obstacle.move_turn(int(goal.order[10:])):
                self._result.result = 'success'
            else:
                self._result.result = 'false'
                self.loger.info("%s false", "move_turn")
            self._as.set_succeeded(self._result)
        
        # elif goal.order == "slam":
        #     self.loger.info("slam_start")
        #     if self.slam_status:
        #         self._result.result = 'slam:already'
        #     else:
        #         self.slam_status = True
        #         if self.ros_c.start_slam():
        #             self._result.result = 'slam:success'
        #         else:
        #             self._result.result = 'slam:false'
        #             self.loger.info("建图节点启动失败")
        #     time.sleep(5)
        #     self._as.set_succeeded(self._result)
        
        elif goal.order == "close_slam":
            self.loger.info("slam_stop")
            output = self.ros_c.stop_slam()
            if output:
                self._result.result = 'close_slam:'+ output
            else:
                self._result.result = 'close_slam:false'
                self.loger.info("close_slam: false")

            self._as.set_succeeded(self._result)

        
        elif goal.order == "nav":
            self.loger.info("nav_start")
            if self.nav_status:
                self._result.result = 'nav:already'
                self.loger.info("nav_start: already_start")
            else:
                self.nav_status = True
                if self.ros_c.start_nav():
                    self._result.result = 'nav:success'
                else:
                    self._result.result = 'nav:false'
                    self.loger.info("nav_start: false")
            time.sleep(2)
            self._as.set_succeeded(self._result)
        
        elif goal.order == "close_nav":
            self.loger.info("nav_stop")
            print(self.nav_status)
            if self.nav_status:
                self.nav_status = False
                if self.ros_c.stop_nav():
                    self._result.result = 'close_nav:success'
                else:
                    self._result.result = 'close_nav:false'
                    self.loger.info("close_nav: false")
            else:
                self._result.result = 'close_nav:already'
                self.loger.info("close_nav: already")
            time.sleep(3)
            self._as.set_succeeded(self._result)
        return
    
    def shutdown(self):
        self.loger.info("closing action_server!")
        return

        
      
if __name__ == '__main__':
  Action_Power("action_server_power")
  rospy.spin()


